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CHAPTER  I 
INTRODUCTION 

A.  Estimation  In  Optimum  Control 

Estimation  techniques  have  become  very  Important  In  modern  control 
theory  applications.  In  many  cases  the  need  for  an  estimation  scheme 
arises  when  optimum  control  is  to  be  applied  to  the  system.  The  optimum 
control  law  is  usually  a  function  of  all  of  the  states  of  the  system. 
However  not  all  of  the  states  are  directly  available.  Therefore  it  is 
necessary  to  extract  information  concerning  the  states  from  the  measure¬ 
ments  which  are  available.  This  process  is  referred  to  as  estimation. 

A  basic  feature  of  any  optimum  estimation  technique  is  that  the 
estimate  of  the  states  is  updated  by  continuously  observing  the  output, 
so  as  to  minimize  some  performance  index  which  is  generally  a  function 
of  the  error  in  estimation  [1].  For  the  Kalman  estimator  [2,  3],  used 
in  this  thesis,  the  variance  of  the  error  in  estimation  is  minimized. 

A  block  diagram  of  an  estimation  process  is  shown  in  Fig.  1.  The 
estimator  is  employed  to  determine  the  optimum  estimate  of  the  state 
variables  from  the  measured  output  variables.  The  controller  is  used 
to  generate  an  optimum  control  law  on  the  basis  of  the  estimate  of  the 
state  variables  [4]. 

As  in  most  problems,  the  optimum  solution  may  not  be  the  most 
economical  or  practical.  Often  it  Is  not  advantageous  to  estimate  all 
of  the  state  variables,  rather  only  those  states  which  have  the  largest 
influence  on  the  control  law.  By  doing  this  the  complexity  of  the 
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Fig.  1  Optimum  estimation  and  control  scheme. 

estimator  and  the  number  of  computations  involved  are  reduced  considerably. 
However  the  degree  of  accuracy  will  be  less  when  compared  to  the  optimum 
estimator. 

B.  Criteria  for  an  Estimation  Scheme 

Various  properties  may  be  used  as  comparison  criteria  for  an  estima¬ 
tion  scheme.  The  three  criteria  considered  here  are: 

1 .  accuracy 

2.  speed  of  the  computation 

3.  complexity. 

All  these  criteria  are  closely  related.  To  obtain  a  high  accuracy,  the 
technique  will  inevitably  be  a  sophisticated  and  complex  one.  Complex 
techniques  are  limited  however,  because  of  the  size  of  the  available 
digital  computer.  The  higher  the  required  accuracy,  the  more  complex  the  tech 
nique  has  to  be  anu  the  bigger  and  more  expensive  the  computer  has  to  be. 
Besides  accuracy,  a  second  important  factor  to  be  considered  is  the  time 
between  measurements.  The  time  it  takes  to  calculate  a  new  estimate 
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depends  on  the  complexity  of  the  technique  and  the  speed  of  the  computer. 
Obviously  the  smaller  this  time  the  better.  When  the  measurement  time 
decreases,  the  discrete  case  approaches  the  continuous  case,  and  more 
information  about  the  states  becomes  available  in  a  shorter  time,  yielding 
a  possibility  that  the  system  reaches  its  steady  state  much  faster. 

In  many  cases  such  as  airborn  navigation  systems,  measurements 
must  be  taken  at  short  intervals  because  of  the  speed  of  the  aircraft 
and  the  short  duration  of  the  flight.  In  marine  navigation  systems  the 
speed  of  computation  is  not  critical  and  often  accuracy  appears  to  be 
less  critical  too.  All  considerations  call  for  an  estimator  which  is  as 
simple  as  possible  to  implement  on  a  special  purpose  digital  computer, 
i.e.,  an  estimator  that  estimates  only  those  states  which  are  necessary 
to  obtain  a  good  control.  Though  the  estimator  may  not  be  optimum 
anymore,  and  the  accuracy  decreased,  the  time  of  computation  may  be 
decreased  sufficiently  to  allow  more  measurements.  This  simplification 
of  the  estimator  results  in  a  smaller  computer,  and  possibly  an  increased 
number  of  measurements  which  makes  up  for  the  lost  accuracy,  as  more 
information  can  be  obtained  in  a  shorter  time.  The  trade-off  between 
time  of  calculations  and  the  complexity  of  the  computer,  however,  has  to 
be  considered  for  every  specific  case. 

This  thesis  develops  a  computer  program  with  which  it  is  possible 
to  simulate  dynamical  systems  described  by  linear  differential  equations 
with  constant  coefficients  and  random  driving  terms,  and  rapidly  to  compare 
and  to  analyze  different  estimation  techniques  with  respect  to  accuracy  and 
computational  complexity. 

Since  the  equations  of  the  system  will  be  written  in  state  variable 
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form,  vector  notation  will  be  used  throughout.  Although  the  estimator 
equations  can  be  written  In  cortlnuous  as  well  as  discrete  form,  only 
the  discrete  equations  are  considered  since  a  digital  computer  simula¬ 
tion  will  be  used. 
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CHAPTER  II 

MATHEMATICAL  SOLUTION  TO  THE  PROBLEM 
A.  Formulation  of  the  Problem 

In  this  chapter  the  necessary  equations  are  presented  that  provide 
a  framework  by  which  It  is  possible  to  study  the  performance  of  the 
optimum  and  sub-optimum  Kalman  estimator  or,  as  It  Is  sometimes  called, 
the  Kalman  filter.  The  differential  equation  describing  the  system  is 

x(t)  =  F  x(t)  +  w( t) 

(2-1) 

y(t)  -  M  x(t)  +  v ( t ) 

where 

* 

x(t)  an  nb-vector  denoting  the  states  in  the  system,  with 
Initial  condition  x(0)  *  x0. 

w(t)  an  nb-vector  of  gaussian,  white  noise  processes  with  zero 
mean 

y(t)  an  m-vector  of  the  outputs  of  the  system 

v(t)  an  m-vector  of  the  errors  (gaussian,  white  noise  sequence) 

F  an  nb  x  nb  system  matrix 
M  an  m  x  nb  measurement  matrix. 

The  number  of  states  In  the  system  is  given  by  nb,  and  the  number  of 
outputs  by  m. 

To  solve  the  problem  numerically,  Eq.  (2-1)  can  be  rewritten  as  a 
difference  equation 

\  =  *„  Vl  *  «-2> 

* 

All  vectors  are  considered  to  be  column  vectors  unless  otherwise 
indicated. 
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where 


n 


t)  w(t)  di 


(2-3) 


where  $n  =  o( tR ,  tn_^)  is  the  transition  matrix,  describing  the  change 
of  the  state  vector  from  time  tn_j  to  time  tn<  Since  w(t)  and  v(t)  are 
random,  x(t)  and  y(t)  will  also  be  random.  Therefore  when  an  estimate 
of  x(t)  is  generated  based  upon  the  measurements  y(t),  this  quantity 
will  be  random.  The  approach  that  will  be  utilized  to  study  these  random 
quantities  will  be  to  investigate  their  covariance  matrices.  That  is 
the  matrix  defined  by 

Cov  [x(t)]  =  E[x(t)xT(t)]  (2-4) 


where  superscript  T  denotes  matrix  transposition  and  E  is  the  expectation 
operator.  In  general,  w(t)  and  v(t)  have  zero  mean,  making  x(t)  and  y(t) 
have  zero  mean  values.  Statistically  the  covariances  of  w(t)  and  v(t)  , 
can  be  described  by 

E[w(t)wT(r)]  =  Q6(t  -  x)  (2-5) 

E[v(ti  )vT(tj)3  =  C5i;j  (2-6) 

where  o(t  -  t)  is  a  Dirac  delta  function  for  continuous  signals  arJ 
is  the  Kronecker  delta  function  for  discrete  signals. 


B.  Optimum  Kalman  Filter 

The  optimum  Kalman  filter  estimates  all  the  states  of  the  system, 
and  uses  the  same  mathematical  model  for  the  filter  as  for  the  system. 

A  block  diagram  of  the  optimum  Kalman  estimator  is  given  in  Fig.  2. 
Defining  the  states  of  the  system  by  x(t)  and  the  estimate  of  the  state 
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by  x(t),  the  error  In  estimation  Is  defined  by 
*{t)  »  x(t)  -  x(t) 

If  an  estimate  Is  provided  at  some  time  tn_^»  and  no  subsequent  measure¬ 
ments  are  available,  the  best  estimate  of  the  states  at  all  subsequent 
times  Is  obtained  by  solving  the  deterministic  portions  of  Eq.  (2-2) 


x„  «  4>  x„  , 
n  n  n-1 


(2-7) 


where  $n  Is  the  same  transition  matrix  used  for  updating  the  states  of 
the  system.  The  estimate  at  tn_j  is  used  as  an  initial  condition.  At 
time  tn  the  errors  In  estimation  will  have  propagated  according  to 

x  -  x  =  $  x  ,  +  , 
n  n  n  n-l  n  n  n-1 


'X,  \ 

X„  *  $  X  .  +  u„ 
n  n  n-1  n 


(2-8) 


The  resulting  value  for  xn  can  be  used  as  the  Initial  condition  for  the 
following  update.  However,  If  a  measurement  Is  obtained  at  time  tn,  a 
new  estimate  can  be  calculated.  The  use  of  measurements  provided  at 
discrete  Instants  of  time  causes  the  error  covariance  to  be  discontinuous, 
having  different  values  before  and  after  the  measurements.  For  this 
reason,  the  error  Inmediately  before  a  measurement  is  designated  x(-) 
and  the  same  error  after  the  measurement  is  x(+).  The  same  notation 
applies  to  the  estimate  and  other  matrices  with  a  discontinuity  at  the 
measurement  time. 

With  the  new  estimate,  the  error  in  estimation  becomes 


V+>  =  *n  -  *n<+> 


Hopefully  the  new  estimate  will  cause  the  error,  or  rather  Its  covariance 
to  be  reduced  In  some  way. 

To  solve  the  problem  statistically,  the  covariance  Is  taken  of 

.  Eq.  (2-8) 

Cov  (xn)  *  *n  Cov  (xn_1)  +  Cov  <un)  (2-9) 

Using  the  notation 


Rn  -  Cov  (un)  and  Pn  -  Cov  (xn) 
Eq.  (2-6)  can  be  written  as 


P„  ■  P„  ,  *'  +  R 
n  n  n-1  n  n 


(2-10) 


The  techniques  which  yield  numerical  solutions  for  «n  and  Rn  are  derived 
in  Appendix  A,  and  are  given  by 


F  ( t  - t  ,) 

41  s  e  n  n_1  -  l 
n  1*0 


F'«„  • 


Qn 


(tn  *  V<>' 


(2-11) 


(2-12) 


where  Qn  Is  given  by 

In  ■  F«n-1  *  <FVl>T  <Z-'3> 

The  initial  condition  for  Qn  Is  given  by  the  covariance  matrix  for  w(t), 
see  Eq.  (2-5).  The  covariance  matrix  of  the  error  In  estimation  is  at 
discrete  time  Instances  sequentially  updated  according  to  Eq.  (2-10). 
This  simulates  the  dynamic  behavior  of  the  covariance  of  the  error  in 
estimation  between  the  measurements,  which  can  now  be  observed  each  time 
the  error  is  updated.  The  covariance  of  the  error  In  estimation  will 
propagate  according  to  Eq.  (2-10)  until  the  time  when  a  measurement  Is 
taken  and  a  new  estimate  is  calculated.  The  updated  estimate  is  given 
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by 


VW+> 


(2-14) 


By  taking  a  measurement  a  correction  term  Axn  is  found  which  gives  a  new 
estimate 


\,w  *  »„(-)  *  a;„ 


(2-15) 


According  to  Ref.  [3]  the  equations  to  calculate  Axn  can  be  written  as 


A*n  =  ^n  * 


(2*16) 


where 

S,  ■  Pn<->  nT[«  p„(-l  "T  ♦  cr'  (2-17) 

and 


yn  =  M  xn  +  v 

Having  found  the  optimum  gain  Kn,  the  new  estimate  is  calculated  according 
to  Eqs.  (2-15)  and  (2-16). 


¥*)  =  *„(->  *  KA  -  M  *„<->]  (2-18) 

The  new  covariance  matrix  of  the  error  is  found  by  taking  the  covariance 
of  Eq.  (2-18)  yielding 

P„M  ■  [I  -  Kn  M]  PnH  [I  -  K„  M]T  t  K„  C  Kj  (2-19) 

This  equation  Is  usually  used  In  its  simpler  but  numerically  equivalent 
form  (see  Ref.  [5]) 


Pn(+)  =  E1  *  Kn  Pn(-)  (2-20) 

The  only  drawback  of  Eq.  (2-20)  is  that  algorithmically  the  equation 
does  not  yield  a  symmetric  matrix.  Round  off  errors  in  the  computer 
cause  the  matrix  to  be  non-symmetric  resulting  in  errors  which  can 
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propagate  rapidly,  especially  with  a  large  number  of  states. 

To  analyze  the  accuracy  of  the  optimum  Kalman  filter  It  Is  only 
necessary  to  study  the  time  behavior  of  P  .  The  equations  for  this  time 
history  are  given  by  Eqs.  (2-10),  (2-17)  and  (2-20).  The  flow  diagram 
of  an  analysis  program  for  this  purpose  is  shown  In  Fig.  3. 

C.  Sub-optimum  Kalman  Filter  with  Control  Matrices 

In  case  of  a  sub-optimum  filter,  the  dynamical  model  used  to  up¬ 
date  the  estimate  Is  different  from  the  dynamical  model  of  the  system. 
Generally,  the  transition  matrix  in  the  filter  Is  of  a  lower  order 
than  the  transition  matrix  of  the  system.  This  is  In  order  to  simplify 
and  reduce  the  computations  involved  In  calculating  a  new  estimate.  The 
matrices  used  in  the  filter  will  be  specified  with  an  asterisk. 

The  differential  equation  used  in  the  filter  to  simulate  the  system 
is  given  by 

x*(t)  ■  F*  x*(t)  +  w*(t)  (2-21) 


which  is  In  discrete  form  written  as 
*  *  *  * 


X  »  ,  +  U 

n  n  n-l  n 


it  it  it 

y  =  M  x  +  v 
•'n  n 


(2-22) 

The  model  of  the  output  process  is 

(2-23) 

The  new  estimate  calculated  at  the  measurement  time  Is  therefore  given  by 

x„(+)  ■  *„(-)  +  KnCyn  _  M*  K{~)]  (2‘24) 

where 

xnH  ■  *„_,<+>  <2-25> 


Flow  diagram  of  an  optimum  Kalman  estimation 


program 
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The  reason  for  doing  this  becomes  obvious  when  the  covariance  of  z  Is 
taken. 


Cov(z)  «  E 


T 

|  -t" 

XX 

1  XX 

"T 

'  ~.7f 

XX 

1  XX 

(2-31) 


By  merely  partitioning  the  Cov(z)  matrix  the  auto-correlations  and 
cross-correlations  of  x  and  x  are  directly  defined.  If  Cov(z)  Is  known, 
the  covariance  of  the  error  can  be  calculated  by  using  Eq.  (2-30). 

However  there  Is  another  method  [6]  which  does  not  require  the 
partitioning  of  the  Cov(z)  matrix.  Use  Is  made  of  two  control  matrices 
A1  and  A2.  With  these  matrices  control  Is  applied  to  the  system  at  the 
measurement  times,  according  to  the  following  equations 


*;(♦)  »  xn(+)  -  A1  in(+)  (2-32) 

xn  "  xn  ’  ^  xn(+)  (2-33) 

where  xn(+)  is  the  new  calculated  estimate  at  the  measurement  time 
before  control  has  been  applied.  The  vectors  x^  and  x^(+)  are  respectively 
the  system  state,  and  the  estimate  after  control  has  been  applied. 

To  simulate  and  observe  the  dynamic  behavior  of  both  the  state  of 

the  system  and  the  estimation  of  the  state,  the  covariance  matrix  of  z 

must  be  updated  at  discrete  Intervals.  The  covariance  of  the  state  is 

given  by  the  matrix  In  the  upper  left  comer  of  the  Cov(z)  matrix  In 

Eq.  (2-31),  and  the  covariance  matrix  of  the  estimate  Is  In  the  lower 

right  corner.  From  Eqs.  (2-2)  and  (2-25)  It  Is  easily  derived  that 

between  measurements  the  relation  of  z_  to  z„  ,  Is  described  by 

n  n- 1 
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4  1 

o" 

—  m 

U_ 

zn 

__r_  i 
0  ! 

* 

•n 

zn-l  + 

_  A 
0 

_  . 

(2-34) 


This  equation  can  be  written  in  the  form 


0_  z  i  +  U 
n  n- 1  n 


(2-35) 


where  en  Is  a  transition  matrix  for  the  augmented  state  and  Un  Is  an 
error  vector  due  to  the  random  driving  terms.  Taking  the  covariance  of 
Eq.  (2-35)  yields 

Cov(zn)  =  on  Cov(zn_1)  oj  +  Cov(Un)  (2-36) 

where  from  Eq.  (2-34)  and  (2-11)  Cov(l)n)  is  found  to  be 


Cov(Un)  = 


.  R  1  0 
L  J2  J - 

0  |  0 


At  the  measurement  time  a  new  "optimum"  gain  Kn  Is  calculated  with 
Eq.  (2-27),  and  a  new  matrix  Pn(+)  is  obtained  from  Eq.  (2-28).  Having 
calculated  a  new  gain  Kn  the  estimate  is  corrected  according  to 


*  *„<->  *  -  n*  *„(-» 


where  yn  is  given  by  the  system  equation 


(2-37) 


y  =  M  x  +  v 
•'n  n 

The  new  estimate  xn(+)  Is  then  fed  back  through  the  control  matrices  to 

x  and  x  (+)  as  described  in  Eqs.  (2-32)  and  (2-33).  By  substituting 
n  n 

Eq.  (2-37)  into  these  two  equations  the  new  values  for  the  state  and  the 
estimate  are 

x;  «  0  -  a2  Kn  «)*„  “  A2(I  '  S,  “  A2  Kn  v  (2'38) 
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*n{+)  *  (I-Al)Kn  M  xn  +  (NAiHl  -  ^  M*)xn(-)  +  (I-A^v  (2-39) 
where  the  prime  denotes  that  control  has  been  applied.  Augmenting 
with  x^  will  give  the  new  value  for  z^.  The  equation  describing  the 
relation  between  z^  and  z^  Is  of  the  same  form  as  Eq.  (2-35) 


z'  «  e1  z  +  u* 
n  n  n  n 

where  Q ^  and  are  given  by 


1 

31  «  - 

L( 


(I  -  V*n  M  !  (I  “  VO  *  S,  M*)J 


r  ~a2  ji  vi 

IT1  -  A'jJvj 


(2-40) 


(2-41) 


(2-42) 


Taking  the  covariance  matrix  of  Eq.  (2-40)  yields 


Cov(z^)  *  Cov(zn)  o^T  +  Cov(U^) 


(2-43) 


where 


Cov(U 


-A,  K  C 


i').  |*A. 1  Ki _A2.  .  _ ; 

n  L(I”A1)Kn  C  "T 


kJaJ  ]  (I-A,)Kn  C  kJO-A,?] 
which  can  also  be  written  In  the  simpler  form 


Cov(U^)  =  K„  C  K l  [-A2T  I 


I  -  A,] 


(2-44) 


D.  Function  of  the  Control  Matrices 

The  function  of  the  control  matrices  Is  shown  In  Fig.  4.  With 
and  A2  equal  to  zero  there  will  be  no  control,  and  the  normal  propagation 
of  the  state  vector  x  and  the  estimate  x  Is  simulated.  With  the  option 
of  having  A^  and  A2  It  is  possible  to  propagate  the  covariance  matrix  of 


Fig.  4  Sub-optimum  estimation  scheme  with  control  matrices. 


X> 
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the  error  In  estimation 
Cov(x)  ■  Cov(x  -  S  x) 
Making 


I  and  A2  *  S,  the  state  vector  and  the  estimate  become 


x '  ■  x  -  S  x_(+)  ■  x  (+) 
n  n  n'  '  n 


x;(+)  -  (I  -  *  0 


(4-45) 

(4-46) 


In  other  words,  the  vector  z^  contains  the  error  In  estimation  In  Its 
upper  part.  Updating  the  covariance  of  z^  will  simulate  the  propagation 
of  the  error  In  estimation. 

The  advantage  of  this  technique  is  that  It  Is  not  necessary  anymore 
to  calculate  the  covariance  of  the  error  separately  from  the  updating  of 
Cov(zn).  With  the  option  of  the  two  control  matrices,  the  covariance  of 
the  error  is  obtained.  In  the  upper  left  corner  of  Cov(zn)  as  Indicated 
above  by  making  ■  0  and  Ag  "  S.  A  block  diagram  for  a  sub-optimum 
filter  is  contained  in  Fig.  4.  A  flow  diagram  for  an  analysis  of  a 
sub-optimum  filter  Is  contained  in  Fig.  5. 


Fig.  5  Flow  diagram  of  a  sub-optimum  Kalman  estimator. 


CHAPTER  III 


DESCRIPTION  OF  THE  PROGRAM 


A.  Introduction 

This  chapter  contains  an  explanation  of  a  computer  program  developed 
for  analyzing  estimation  schemes  with  a  Kalman  filter,  using  state 
variable  techniques.  The  program  utilizes  the  equations  derived  In 
Chapter  II  for  the  sub-optimum  Kalman  estimator  with  control  matrices. 

A  complete  listing  of  the  program  Is  contained  In  the  Appendices. 

The  program  was  written  In  Fortran  IV  for  the  CDC  3600.  Variable 
dimensions  are  used  throughout  the  program  except  for  the  dummy  arrays, 
which  are  used  only  for  temporary  storage  to  perform  certain  matrix 
operations.  These  temporary  matrices  denoted  by  T1,  T2,  etc.,  are  In 
COMMON.  This  Is  done  because  every  subroutine  requires  a  different 
number  of  dummy  arrays  of  different  sizes.  The  COMMON  can  then  be 
arranged  to  suit  the  need  of  every  subroutine.  All  other  arrays  have 
variable  dimensions,  which  makes  It  convenient  to  change  the  storage 
assigned  to  these  matrices  by  only  changing  the  DIMENSION  statement  In 
the  main  program.  Changing  dimensions  becomes  Important  when  the  memory 
space  available  In  the  computer  Is  limited.  Certain  constant  matrices 
which  do  not  need  many  computations  to  compute,  are  recalculated  with 
every  measurement  In  order  to  use  their  storage  space  for  the  calculation 
of  other  matrices.  This  way,  without  losing  much  time  a  great  deal  of 
memory  space  Is  obtained.  With  this  program  it  Is  possible  on  a  computer 
with  32k  memory  locations  to  analyze  estimation  schemes  with  up  to  25 
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states,  25  estimates  and  10  outputs,  or  any  other  combination  which 
results  In  an  equal  amount  of  storage.  However,  the  DIMENSION  statement 
In  the  main-line  program  (and  the  COMMON)  has  to  be  changed  accordingly. 
The  different  subroutines  used  In  the  program  and  how  they  are  linked 
together  are  shown  In  Fig.  6. 


B.  Data  Deck  Setup 

The  main-line  program  called  ESTIM,  starts  by  reading  all  the  non- 
dlmensloned  variables  used  In  the  program.  These  variables  are  to  be 
punched  on  the  first  data  card  which  will  be  read  according  to  the 
following  read  statements 
1  F0RMAT(8I5,3E10.5) 

READ  1,  N,NB,M,KT,KTF,KMAX,IOUT,KAI ,T,AE,RE 


where 

N  the  number  of  states  In  the  filter 

NB  the  number  of  states  In  the  system 

M  the  number  of  elements  In  the  output 

KT  the  number  of  updates  between  measurements 

KTF  the  number  of  measurements  to  be  taken 

KMAX  the  maximum  number  of  Iterations  allowed  In  the  MEXP  subroutine 
for  the  calculation  of  the  transition  matrices  and  the  random 
error  term  of  the  matrix  Rlcattl  equations 

I0UT  a  flag  used  In  the  print  routine  OUT,  and  set  equal  to: 

1  when  the  square  root  of  only  the  diagonal  terms  of  PI  and 
COVZ  are  to  be  printed  as  output,  or 

2  when  the  full  matrix  of  PI  and  COVZ  are  to  be  printed  out. 

KAI  a  flag  which  Is  used  In  the  subroutine  AINPUT,  and  set  equal  to: 

1  when  only  the  diagonal  terms  of  AQ,  BQ,  PI,  and  COVZ  are 
used  »s  Input 

2  when  the  diagonal  and  sub-diagonal  terms  of  these  symmetric 
matrices  are  to  be  read 


T  the  time  between  measurements.  In  the  same  time  unit  as  used 
In  the  system  matrices 

AE  and  RE  are  respectively  the  absolute  and  the  relative  error  which 
are  allowed  In  the  calculation  of  PHI,  BPHI,  RK,  BRK 

The  next  set  of  data  cards  contain  sequentially  the  following  matrices 

AF  an  n  x  n  matrix  which  Is  used  as  model  of  the  system  In  the 
filter 

BF  an  nb  x  nb  matrix  representing  the  model  of  the  system 

AQ  an  n  x  n  covariance  matrix  of  the  random  driving  terms  in 
the  model  of  the  filter 

BQ  an  nb  x  nb  covariance  matrix  of  the  random  driving  terms  In 
the  model  of  the  system 

AH  an  m  x  n  measurement  matrix  of  the  states  In  the  filter 

BM  an  m  x  nb  measurement  matrix  of  the  states  In  the  system 

AC  an  m  x  m  covariance  matrix  of  the  noise  at  the  output  of 

the  filter 

BC  an  m  x  m  covariance  matrix  of  the  noise  at  the  output  of  the 
system 

PI  an  n  x  n  covariance  matrix  of  the  error  In  estimation 

COVZ  an  (nb+n)  x  (nb+n)  covariance  matrix  of  the  augmented  vector 

of  the  states  In  the  system  and  the  estimated  states  In  the 
filter 

AA  an  n  x  n  control  matrix  feeding  back  the  estimate  after  the 
measurement  according  to  Eq.  (2-32) 

BA  an  nb  x  n  control  matrix  feeding  back  the  estimate  after  the 
measurement  to  the  states  of  the  system  (see  Eq.  2-33). 


C.  Input  Formats 

The  matrices  are  read  In  row-wise  according  to  F0RMAT(8E10.4). 


If  the  number  of  elements  In  a  row  is  greater  than  the  number  of  fields 
specified  by  the  format  statement,  l.e.,  eight  In  this  case,  the  reading 
of  the  elements  Is  continued  on  the  following  data  card(s).  The  next 


row  Is  started  at  a  new  data  card,  which  continues  until  all  rows  are 
read.  An  example  of  the  statements  used  for  reading  a  matrix  A  with 
dimensions  n  x  m  Is  given  by 
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1  F0RMAT(8E10.4) 

DO  9  I-l.N 

9  READ  1.(A(I,J),J-1,M) 

The  reading  of  AQ,  BQ,  PI,  COVZ  Is  slightly  different.  Here 
advantage  Is  taken  of  the  fact  that  these  matrices  are  symmetric.  There* 
fore  only  the  elements  of  the  lower  triangle  Including  the  elements  on 
the  diagonal  are  read.  Again  the  reading  Is  done  row-wise  as  was  the 
case  with  a  non-symmetrlc  matrix.  The  only  difference  Is  that  now  the 
diagonal  term  Is  considered  to  be  the  last  element  In  the  row.  Inside 
the  subroutine  AINPUT  the  upper-diagonal  terms  are  equated  to  their 
corresponding  sub-diagonal  terms.  The  statements  to  read  a  symmetric 
matrix  B  with  dimension  n  x  n  are 

1  FORMAT(8E10,4) 

DO  8  1=1  ,N 

8  READ  1,  (B(I,J),J«1,I) 

A  third  type  of  matrix  is  the  diagonal  matrix  AA.  The  diagonal 
elements  are  put  In  order  on  the  same  data  card,  or  subsequent  data 
cards  If  the  number  of  elements  of  the  diagonal  is  greater  than  8.  All 
off-diagonal  terms  are  set  to  zero  inside  the  subroutine  AINPUT.  The 
statements  used  for  reading  a  diagonal  matrix  C  with  dimensions  n  x  n 
are 

1  F0RMAT(8E10.4) 

READ  1,  (C(I,I),I=1  ,N) 

In  most  cases  the  off-diagonal  terms  of  AQ,  BQ,  PI  and  COVZ  are  also 
equal  to  zero.  The  flag  KAI  provides  the  option  to  read  only  the 
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diagonal  terms  of  these  matrices;  in  the  same  May  as  for  AA. 

As  a  final  check  if  the  data  was  presented  and  read  correctly,  all 
the  variables  and  matrices  that  are  read  as  input  data,  are  directly 
printed.  The  printing  of  the  matrices  Is  done  In  a  separate  subroutine 
DRUK,  so  that  the  print  format,  can  easily  be  changed. 

D.  General  Description  of  the  Program 

After  all  the  data  Is  read  and  printed,  the  transition  matrices 
and  the  error  covariance  matrices  a: e  calculated  in  the  subroutine  MEXP. 

The  accuracy  desired  for  these  matrices  is  to  be  defined  by  AE  and  RE, 
the  absolute  and  relative  error  terms  respectively.  The  iterations 
continue  until  all  elements  in  the  matrix  EXPTA  satisfy  the  inequality 

lAEXPTA^I  <  AE  +  RE  | EXPTA. j|  (3-1) 

where  EXPTA  Is  the  desired  matrix  and  cEXPTA  is  the  last  calculated  term 
of  the  series.  When  Eq.  (3-1)  applies  for  each  element  of  aEXPTA,  the 
total  number  of  iterations  Is  printed  and  the  final  matrix  EXPTA  is 
printed  by  calling  DRUK.  KMAX  provides  a  limit  on  the  number  of 
iterations  allowed.  In  the  case  a  matrix  does  not  converge  within  the 
allowable  number  of  Iterations,  an  error  message  will  be  printed  and 
further  execution  of  the  program  terminated. 

Following  the  calling  of  MEXP,  control  Is  transferred  to  subroutine 
ERCOV  where  the  matrices  PI  and  COVZ  are  updated  and  corrected  at  each 
measurement.  The  way  the  system  and  the  filter  are  updated  and  measure¬ 
ments  are  taken  is  Illustrated  in  Fig.  7.  The  Initial  conditions  for  PI 
and  COV7  are  specified  by  the  input  data.  The  first  update  is  at  t  =  T/KT, 
the  states  in  the  filter  and  the  system  are  updated  without  the  correction 
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Fig.  7  Propagation  of  the  states, with  measurements. 

k(  the  estimate  by  the  Kalman  filter.  The  number  of  updates  does  not 
have  any  effect  on  the  propagation  of  the  states;  it  merely  provides  a 
possibility  to  observe  the  states  at  certain  times  between  measurements. 
At  every  update  the  corresponding  time  is  printed,  and  PI  and  COVZ  are 
printed  in  subroutine  OUT.  According  to  the  value  of  the  flag  IOUT,  Pi 
and  COVZ  are  either  printed  completely  or  the  square  root  of  only  the 
diagonal  terms  is  printed. 

After  KT  updates  have  been  calculated  and  printed  out,  the  time  will 
be  t  =  T,  which  is  equal  to  the  measurement  time.  The  number  of  the 
measurenent  Is  printed  and  the  control  is  transferred  to  subroutine 
FILTER.  Here,  the  new  optimum  gain  AK  is  calculated  according  to  Eq. 
(2-27)  and  printed.  With  the  new  gain,  PI  is  corrected  according  to 
Eq.  (2-28).  When  control  returns,  COVZ  is  implemented  with  the  new 
estimate  as  described  by  Eq.  (2-43).  The  new  PI  and  COVZ  are  printed 
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again  in  subroutine  OUT.  Control  returns  to  the  beginning  of  ERCOV, 
and  the  procedure  of  updating  and  taking  a  measurement  continues. 

After  KTF  measurements  have  been  taken,  control  returns  to  the  main-line 
program. 

The  program  is  Implemented  with  a  DO-loop  to  accept  up  to  ten  data 
decks.  An  "END  OF  FILE"  (EOF)  check  Is  performed  and  execution  of  the 
program  is  terminated  when  the  EOF  card  Is  encountered  in  the  case  of 
less  than  ten  data  decks.  The  first  card  of  the  new  data  deck  follows 
directly  after  the  last  card  of  the  preceding  data  deck. 

E.  Explanation  of  the  Subroutines 

ESTIM:  The  main-line  program,  ESTIM,  is  kept  quite  simple.  A 
DO-loop  with  a  dumn\y  variable,  II,  gives  the  possibility  of  accepting 
up  to  ten  data  decks.  Inside  the  DO-loop,  the  first  data  card  with  ail 
the  variables  is  read  as  explained  in  the  beginning  of  this  chapter. 

With  the  following  statement,  CALL  AINP'JT,  all  input  matrices  are  read. 
After  control  returns,  the  subroutine  MEXP  Is  called  four  times;  twice 
with  the  flag  KLM  ■  1 ,  to  calculate  the  transition  matrices  PHI  and 
BPHI ,  respectively  of  the  filter  and  the  system;  and  two  times  with  the 
flag  KLM  =  2,  for  the  calculation  of  the  error  covariance  terms  cf  the 
filter  and  the  system,  designated  RK  and  BRK.  These  call  statements  are 
followed  by  a  call  statement  for  subroutine  ERCOV  where  the  rest  of  the 
calculations  are  performed.  After  the  execution  of  this  statement, 
control  goes  back  to  the  beginning  of  the  DO-loop  and  reads  the  first 
data  card  of  the  next  data  deck.  With  the  "END  OF  FILE"  check 
following  the  read  statement,  the  program  is  terminated  when  the  EOF 


card  Is  encountered  In  the  case  of  less  than  ten  data  decks. 


Subroutine  AINPUT:  In  this  subroutine  the  following  matrices  are 
read:  AF,  Br,  AQ,  BQ,  AM,  BM,  AC,  BC,  PI,  COVZ,  AA,  BA  according  to 
the  format  explained  in  the  beginning  of  this  chapter.  This  subroutine 
is  called  with 

AINPUT(AF,BF,AQ,BQ,AM,BM,AC,BC,P1 ,COVZ,AA,BA,N,NB,M,NNB,KAI) 

where  KAI  is  the  flag  to  select  the  read  format  of  the  matrices  and 
NNB  is  the  number  of  states  in  the  augmented  state  vector,  2. 

Sibroutlne  MEXP:  Referring  to  the  matrix  exponential  and  the  error 
covariance  flow  diagrams  in  Appendix  A,  it  can  be  noted  that  a  large 
similarity  exists  between  both  diagrams.  Consequently,  both  programs 
have  been  combined  into  one  called  MEXP.  The  flow  diagram  of  this  sub¬ 
routine  Is  shown  in  Fig.  8.  According  to  the  value  of  the  flag  KLM, 
either  the  equations  for  the  matrix  exponential  or  the  error  covariance 
routines  are  used.  When  KLM  =  1  the  transition  matrix  is  computed  and 
with  KLM  *  2  the  error  covariance  matrix  is  calculated.  Both  matrices 
are  required  to  update  the  covariance  matrix  of  the  states  according  to 
Eq.  (2-26). 

In  order  to  be  able  to  acquire  a  high  accuracy  with  slowly  con¬ 
verging  matrices  many  iterations  are  necessary.  With  large  matrices 
this  Involves  many  computations  which  result  In  an  undesirable  growth 
of  the  truncation  errors  In  the  computer.  Double  precision  is  used 
for  the  algorithm  to  circumvent  this  problem.  The  parameter  list  in 
this  subroutine  is 
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MEXP{D,F,A,T1 ,T2 ,EXPTA ,T3 , N , KMAX  ,AE , RE , KLM) 

The  following  variables  are  to  be  specified  In  the  calling  program 
D  the  time  between  updates,  D  *  T/KT 
F  the  system  matrix 

A  either  the  system  matrix  F,  when  KLM  ■  1,  or  the  covariance 
of  the  random  driving  terms  Q,  when  KLM  ■  2. 

N  the  order  of  the  matrices 

KMAX  the  maximum  number  of  Iterations 

AE  the  absolute  error 

RE  the  relative  error 

KLM  a  flag 

T1 ,  T2  and  EXPTA  are  double  precision  arrays  which  are  used  only  for 
temporary  storage  Inside  the  subroutine.  EXPTA  is  the  desired  matrix 
in  double  precision  which  at  the  end  of  all  computations  is  equated 
with  the  single  precision  matrix  T3  which  Is  transferred  to  the  calling 
program  through  the  parameter  list.  The  call  statement  for  calculating 
a  transition  matrix  PHI  might,  for  example,  be 

CALL  MEXP(D,F,F,T1 ,T2,T3,PHI,N,KMAX,AE,RE,1 ) 

The  equivalent  statement  for  the  calculation  of  the  error  covariance 
term  RK  would  be 

CALL  MEXP(D,F,Q,T1  .TZ.TS.RK.N.KMAX.AE.RE^) 

Generally  the  subroutine  operates  as  follows:  The  single  precision 
variable  D  is  equated  to  the  double  precision  variable  T.  Next,  the 
statements  for  EXPTA  *  T1  =  A*T  are  executed.  For  KLM  -  1  the  Identity 
matrix  I  is  added  to  this  first  term  of  the  series.  The  double  precision 
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variable  K,  denoting  the  number  of  Iterations,  Is  Incremented  by  1. 

The  following  term  of  either  series  Is  calculated  and  stored  In  T1  and 
added  to  the  series.  All  elements  of  T1  are  checked  according  to 
Eq.  (3-1).  A  check  follows  to  determine  If  the  number  of  Iterations  K, 
has  exceeded  the  limit  KMAX.  If  one  of  the  elements  of  T1  does  not 
satisfy  Eq.  (3-1)  and  K  Is  still  below  Its  limit,  the  Iterations  continue 
and  the  next  term  of  the  series  Is  calculated.  In  the  case  that  the 
series  does  not  converge  and  the  number  of  Iterations  reaches  Its  limit 
KMAX,  the  message  "number  of  Iterations  exceeded”  will  be  printed  and 
the  execution  of  the  program  terminated. 

Subroutine  ERCQV:  In  this  subroutine  PI  and  COVZ  are  updated  and 
after  each  measurement  incremented  with  a  new  estimate.  The  flow 
diagram  of  ERCOV  Is  represented  in  Fig.  9.  The  parameter  list  in  the 
subroutine  Is  given  by 

ERC0V(PHI ,BPHI ,RK,BRK,AM,BM,AK,AC,BC,P1 ,0H,C0VU,C0VZ,AA,BA,TA,TC, 

N.NB ,M,NNB,KT,KTF,IOUT,D) 

Except  for  AK,  OH,  COVU,  TA  and  TC  all  variables  and  arrays  are  to  be 
specified  In  the  calling  program.  The  matrices  which  have  not  yet  been 
defined  are 

OH  the  transition  matrix  of  the  augmented  state  vector  z 

COVU  the  error  covariance  term  for  the  augmented  z-vector 
TA  and  TC  two  dummy  arrays  with  variable  dimension. 

The  subroutine  starts  with  storing  of  AA  -  I  In  AA,  where  I  Is  the 
Identity  matrix.  This  is  done  because  AA  appears  always  In  conjunction 
with  I.  After  these  steps,  control  comes  to  the  first  DO-loop  where  KTF 
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denotes  the  number  of  measurements  to  be  taken.  Inside  the  DO-loop 
f>n  and  Cov(Un )  are  defined  according  to  Eg.  (2-34)  and  stored  under  the 
names  OH  and  COVU  respectively.  Though  these  two  matrices  are  constant, 
they  are  recalculated  at  each  Measurement  due  to  the  fact  that  they 
share  the  same  storage  locations  is  and  Cov(U^).  This  way  OH  and 
COVU  can  be  used  for  two  purposes  which  savas  useful  storage  space. 
Time-wise,  this  Is  Justified  as  there  are  hardly  any  computations 
Involved  in  calculating  v  and  Cov(Un).  The  following  DO-loop  updates 
PI  and  COVZ  according  to  tgs,  (2*11)  a  ' I -II).  KT  denotes  the  number 
of  updates  between  meas  iremants.  left  ’  each  update  the  propagation  time 
Is  printed  out.  PI  and  COVZ  are  printed  In  subroutine  OUT  after  each 
update.  With  the  system  updated  KT  times  up  to  the  measurement  time  T, 
a  new  measurement  will  be  taken  by  transferring  control  to  subroutine 
FILTER,  where  a  new  AK  and  P!  are  calculated.  In  the  statements 
following  the  return  of  control  from  FILTER,  and  Cov(U^)  are  calculated 
according  to  Eqs.  (2-41)  and  (2-44)  and  are  stored  In  OH  and  COVU.  OH 
Is  obtained  by  first  calculating  the  partitioned  matrices,  and  augmenting 
these  matrices  In  subroutine  SUM.  COVU  Is  obtained  by  augmenting  BA 
with  AA  -  I,  and  post-multi plying  and  premultl plying  AK*BC*AKT  by  the 
newly  obtained  matrix  according  to  Eq.  (2-44).  As  this  augmented  matrix 
contains  only  two  matrices  In  this  rase,  there  is  an  entry  SUMB  provided 
In  subroutine  SUM  which  augments  two  matrices  instead  of  four  as  is 
explained  in  the  description  of  subroutine  SUM.  The  new  matrices  PI  and 
COVZ  are  printed  In  subroutl^p  1UT  and  the  whole  sequence  of  updating 
and  calculating  a  new  estimate  Is  repeated  again  until  the  final  measure¬ 
ment  KTF  is  reached. 
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Subroutine  FILTER:  This  subroutine  of  which  the  flow  diagram  Is 
shown  In  Fig.  10  calculates  the  optimum  gain  AK  and  the  new  error  In 
estimation  PI.  The  parameter  list  In  the  subroutine  and  In  the  calling 
program  Is 

FILTER(AM,AK,P1 ,AC,N,M) 

The  equations  used  for  the  Kalman  filter  are  strictly  according  to  the 
equations  derived  In  Chapter  II,  Eqs.  (2-27)  and  (2-28).  All  variables 
except  AK  are  to  be  defined  In  the  calling  program.  PI  which  Is  trans¬ 
ferred  to  FILTER  Is  equal  to  the  error  In  estimation  at  the  end  of  the 
last  update  when  transferred  back  to  the  calling  program  PI  will  be 
equal  to  the  new  corrected  estimate  according  to  Eq.  (2-28).  The  function 
of  the  statements  can  easily  be  seen  without  further  explanation. 

Subroutine  OUT:  In  this  subroutine  both  PI  and  COVZ  are  printed 
after  each  update  and  after  each  measurement.  The  parameter  list  in 
the  subroutine  and  in  the  calling  program  is 

OUT(  PI  ,C0VZ ,T1 ,N ,NB ,NNB , I0UT ) 

T1  Is  a  dummy  array  used  for  storing  the  square  roots  of  the  diagonal 
elements  of  PI  and  COVZ.  According  to  the  flag  I0UT  either  the  full 
matrix  of  PI  and  COVZ  is  printed  when  I0UT  =  2,  or  the  square  root  of 
just  the  diagonal  terms  is  printed  when  I0UT  =1.  With  merely  changing 
this  small  subroutine  the  print  format  can  easily  be  changed  without 
changing  the  binary  deck  of  a  large  subroutine. 

Subroutine  DRUK:  This  subroutine  is  made  to  print  a  matrix  and 


is  called  by 
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Calculate  new  optimum  gain  AK 


Calculate  new  covariance  matri 
of  the  error  in  estimation,  PI 


Fig.  10  Flow  diagram  of  subroutine  FILTER. 
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ORUK(B.M.N) 

where 

B  Is  the  matrix  to  be  printed 
N  the  number  of  columns 
M  the  number  of  rows. 

The  matrix  A  Is  printed  row-wise,  or  In  case  the  number  of  rows  Is 
larger  than  the  number  of  columns,  the  matrix  is  automatically  printed 
column-wise.  The  message  "transpose"  Is  then  printed  above  the  matrix. 
The  print  format  used  with  the  print  statement  is 

2  FORMAT ( 1 X » 1 2E1 1 .4) 

DO  3  I-l.N 

3  PRINT  2,  (B( I ,J) ,J*1 ,M) 

The  print  format  or  statements  are  easily  changed  to  suit  one's  purpose. 

Subroutine  SUM  and  entry  SUMB:  In  this  subroutine  an  augmented 
matrix  is  obtained  from  four  smaller  matrices  according  to 

Ta  '  b1 


The  call  statement  Is 

SUM(A,B,C. D,$, II, 12, J1.J2, 112,012} 

where 

11  Is  the  number  of  rows  In  A  and  B 

12  is  the  number  of  rows  In  C  and  D 

J1  Is  the  number  of  columns  In  A  and  C 

J2  is  the  number  of  columns  In  B  and  D 

112  is  the  number  of  rows  In  S 

J12  Is  the  number  of  columns  in  S 
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With  the  entry  SUMS  the  following  type  of  augmented  matrix  Is  obtained 

S  ■ 

The  same  parameter  list  Is  used  for  SUMB  as  for  SUM,  only  B,  0,  and  02 
do  not  have  any  significance. 

Subroutine  AOI:  In  this  subroutine  all  elements  of  a  matrix  B 
are  equated  to  zero  when  the  flag  KK  -  1.  Otherwise,  the  matrix  B  Is 
equated  to  the  Identity  matrix.  The  call  statement  1$ 

A0I(B,N,M,KK) 

where 

N  Is  the  number  of  rows  In  B 
M  Is  the  number  of  columns  In  B 
KK  Is  a  flag. 

Subroutine  ABT:  This  subroutine  performs  the  following  matrix 
product  j  ■  A*B^.  The  call  statement  Is 

ABT(A,B,S,K,L,M) 

where 

K  Is  the  number  of  rows  In  A  and  $ 

L  1r  the  number  of  columns  In  A  and  B 

M  is  the  number  of  rows  In  B  and  the  number  of  columns  In  S. 

Subroutine  ABTAC:  This  subroutine  performs  either  of  the  following 
two  matrix  operations 

S  *  A*B*AT  when  the  flag  KK  =  2 

S  =  A*B*CT  when  the  flag  KK  =  3. 
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The  call  statement  is 

ABTAC(A,B,C,T,S»K,L,M,N,KK) 

where 

T  is  a  dummy  array  used  to  store  the  product  of  A  and  B 

K  is  the  number  of  rows  in  A,  T,  S 

L  Is  the  number  of  rows  in  Band  the  number  of  columns  in  A 

M  is  the  number  of  columns  in  B,  C,  T 

N  is  the  number  of  rows  in  C  and  the  number  of  columns  in  S. 

In  the  case  that  KK  =  2,  the  calling  sequence  is 

ABTAC(A,B,A,T,S,K,L,L,K,2) 

Advantage  has  been  tar.en  of  the  fact  that  S  is  symmetric  in  this  case. 
Therefore  only  the  upper-diagonal  terns  including  the  diagonal  terms 
are  calculated,  and  the  sub-diagonal  terms  are  equated  to  the  correspond¬ 
ing  elements  above  the  diagonal.  This  way  calculation  time  is  saved. 

The  following  matrix  subroutines  were  Implemented  on  the  computer 
as  library  subroutines.  Therefore,  these  subroutines  are  not  contained 
in  the  listing  and  only  the  calling  sequence  is  explained 

Subroutine  MATINV:  This  subroutine  is  used  to  take  the  inverse  of 
a  matrix.  Jordan's  method  is  used  to  reduce  a  matrix  A  to  the  identity 
matrix  I  through  a  succession  of  transformations.  With  this  method,  the 
matrix  equation  A*X  =  B  is  solved,  where  A  is  a  square  coefficient  matrix 
and  B  is  a  matrix  of  constant  vectors.  The  inverse  and  the  determinant 
of  A  are  also  computed.  The  calling  sequence  is 

CALL  MATINV(A,N,B,NB,D£T,MA) 


where 


A  is  a  square  matrix  of  which  the  inverse  has  to  be  taken,  as 
output  it  contains  A“1 

N  is  the  order  of  A 

B  is  a  two-dimensional  array  (not  usually  square),  as  output 
contains  X 

N8  Is  the  number  of  columns  In  B;  if  NB  *  0,  the  routine  Is 
used  only  for  the  inversion 

D£T  is  the  determinant  of  A  calculated  by  MATINV 

MA  is  the  dimension  of  A  in  the  calling  program. 

Subroutine  MATSUB:  In  this  subroutine  two  matrices  are  subtracted 
according  to  C  *  A  -  B.  The  calling  sequence  for  this  matrix  routine  is 

Call  MATSUB{A,B,C,N,NX,M) 

where 

N  is  the  number  of  rows  in  A,  B,  and  C 

NX  is  the  row  dimension  of  the  matrices  in  the  calling  program 

M  is  the  number  of  columns  of  A,  B,  and  C 

Subroutine  MATAOD:  In  this  subroutine  two  matrices  are  added 
according  to  C  *  A  +  B.  The  calling  sequence  is  given  by 

CALL  MATADD(A,B,C,N,NX,M) 

where 

N  is  the  number  of  rows  in  A,  B,  and  C 

NX  is  the  row  dimension  of  the  matrices  In  the  calling  program 

M  is  the  number  of  columns  In  A,  8,  and  C 

Subroutine  MATMPV:  In  this  subroutine  two  matrices  are  multiplied 
according  to  C  =  A*B.  The  calling  sequence  Is  given  by 
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CHAPTER  IV 

AN  INERTIAL  ESTIMATION  PROBLEM 

A.  Errors  Occurring  In  an  Inertial  Guidance  System 

In  vehicles  navigated  by  an  inertial  system  It  is  desirable  that 
the  system  computes  the  position  and  velocity  with  respect  to  earth 
very  accurately.  However,  several  types  of  errors  occur  In  an  Inertial 
guidance  system.  These  errors  fall  Into  two  categories 

1.  deterministic 

2.  random 

The  deterministic  errors  are  usually  simple  In  form  and  quite  easy  to 
describe  mathematically,  such  as  errors  with  constant  coefficients  or 
with  sinusoidal  characteristics.  These  errors  are  generally  compensated 
for,  l.e.,  effectively  subtracted  out  of  the  system.  The  random  errors 
are  treated  statistically  based  upon  a  mathematical  specification.  Gyros, 
accelerometers,  initial  alignments,  servos,  digital  or  analog  computers 
and  geographical  data  are  some  examples  of  error  sources  that  arise  either 
within  the  Inertial  system  or  with  outside  data  used  by  the  inertial 
system.  When  dealing  with  errors  in  an  error  analysis  It  Is  necessary  to 
describe  these  errors  mathematically  In  order  to  study  their  propagation. 
Generally,  these  errors  do  not  consist  of  pure  white  noise,  but  are 
correlated  In  time.  This  problem  will  be  solved  by  adding  states  to  the 
state  vector  x,  and  simulating  the  errors  as  being  the  outputs  of 
stochastic  processes  with  white  noise  Inputs. 

It  Is  Impossible  to  Implement  Inertial  guidance  systems  without 
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errors.  These  errors  will  g»*ow  very  large  after  a  period  of  time  If 
they  are  not  corrected.  In  order  to  keep  the  errors  generated  In  an 
Inertial  system  within  acceptable  bounds  It  Is  necessary  to  recalibrate 
the  system  periodically.  The  correction  of  the  system  errors  Is  achieved 
by  the  use  of  independent  sources  of  Information.  These  external  measure¬ 
ments  can  Include  position,  velocity,  attitude  and  combinations  thereof. 
T^e  external  measurements  are  compared  to  corresponding  quantities 
Indicated  by  the  Inertial  system.  The  Kalman  filter  uses  the  differences 
between  indicated  and  measured  quantities  to  provide  the  optimum  estimate 
of  the  errors  in  the  system.  A  block  diagram  of  a  recalibration  scheme 
Is  contained  In  Fig.  11 . 

As  well  as  estimating  the  states  of  the  system,  it  Is  also  important 
to  obtain  with  the  Kalman  filter  an  estimate  of  those  error  sources  which 
are  correlated  in  time.  If  the  error  sources  contain  only  white  noise, 
estimating  the  errors  would  not  assist  In  predicting  the  error  at  the 
next  time  of  interest  due  to  the  fact  that  white  noise  Is  not  correlated 
In  time.  However,  when  the  disturbances  and  the  measurement  errors 
are  not  changing  rapidly  compared  with  the  system  state  and  measure¬ 
ments,  the  filter  accuracy  can  be  enhanced  by  estimating  these  errors. 

The  estimation  of  the  system  disturbances  a  j  sieasurement  errors  which 
have  significant  correlation  time,  increases  the  number  of  state 
variables  to  be  estimated.  This  Is  frequently  described  as  "state 
vector  augmentation." 

In  the  navigation  problem  to  be  considered,  three  types  of  random 
variables  are  used 

1 .  white  noise 


2.  random  constant 

3.  exponentially  correlated  random  variable. 

Of  the  three  types  of  random  signals,  only  the  white  noise  Is  uncor¬ 
related  In  time. 

The  white  noise  Is  denoted  by  w.  The  characteristics  are:  an 

2 

expected  value  E(w)  ■  0,  and  an  autocorrelation  R(t)  ■  o  <s(t). 

A  random  constant  can  be  generated  with  the  use  of  one  additional 
state.  This  Is  Illustrated  In  Fig.  12.  The  sta.e  differential  equation 
can  be  written  as 
e  ■  0 

The  Initial  condition  Is  chosen  according  to  the  nature  of  the  error. 

— 

The  autocorrelation  Is  e  (0). 

The  exponentially  correlated  random  variable  Is  frequently  a  use¬ 
ful  representation  of  errors  In  Inertial  navigation  systems.  The  auto¬ 
correlation  function  of  the  random  signal  Is  a  declining  exponential 

E[e(t,)  e(t2)]  =  o2  e  21  (4-2) 

2 

where  n  Is  the  variance  and  8  is  the  reciprocal  of  the  correlation  time 
An  exponentially  correlated  random  variable  can  be  generated  by  passing 
an  uncorrelated  signal,  i.e.,  white  noise,  through  a  linear  first-order 
feedback  system.  A  block  diagram  for  this  stochastic  process  Is  shown 
In  Fig.  13.  The  differential  equation  of  the  additional  state  variable 
is 

e  a  -8  e  +  w 


where 


(4-3) 


Fig.  11  Block  diagram  of  a  recalibration  scheme. 


Fig.  13  Stochastic  process  with  exponentially  correlated  output. 
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E(w)  *  0,  E[w(t)  w(t  +  t)]  ■  2  e  6(t) 

B.  Simulation  of  an  Inertial  Guidance  System 

The  model  of  the  system  contains  seven  states,  not  Including  the 
augmented  states  for  the  errors.  The  block  diagram  of  the  system  Is 
shown  In  Fig.  14,  see  [7].  This  system  Is  called  a  coupled  model  as  all 
states  are  coupled  resulting  In  a  rather  complicated  F-matrix.  For  the 
optimum  Kalman  filter  the  same  model  Is  used  In  the  filter  as  In  the 
system.  In  order  to  obtain  a  less  complicated  Kalman  filter,  the 
equations  are  simplified,  resulting  In  two  other  models  of  the  system 
as  shown  In  Figs.  15  and  16.  The  reference  frame  used  Is:  x-north, 
y-east,  z-down. 

It  Is  assumed  that  the  accelerometer  and  gyro  errors  can  be  repre¬ 
sented  by  an  exponentially  correlated  random  variable,  Eq.  (4-3).  For 
this  example  only  velocity  measurements  are  considered,  with  measurement 
errors  consisting  of  white  noise  with  a  random  bias  term,  Eq.  (4-1).  The 
state  vector  used  In  all  systems  therefore  is  given  by 


XT  ■  [6RX.  6Ry,  «RX.  6Ry,  <y  *z.  V  vyi  ex,  ey,  ez,  vx.  vy] 

where  6R  Indicates  position,  ♦the  platform  tilt,  ?the  position  error, 
e  the  tilt  error,  and  v  the  measurement  error.  The  differential 
equations  for  the  errors  [8]  are  given  by 


(4-4) 


V  «  -8V  v  +  w7 

(4-5) 

e  s  -8  e  +  W£ 

(4-6) 

0  =  0 

(4-7) 

Fig.  14  Block  diagram  of  an  inertial  navigation  system.  Model  A. 


Fig.  15  Block  diagram  of  an  inertial  navigation  system.  Model  B, 
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For  computational  simplicity,  the  F-matrlx  is  assumed  to  be  non¬ 
varying  with  time,  since  the  change  in  F(t)  is  negligible  over  the  time 
interval  studied.  This  means  that  the  radius  the  earth,  R,  and  the 
gravity  vector,  g,  are  constants,  and  motion  relative  to  the  earth  is 
neglected.  The  model  shown  in  Fig.  14  Is  used  for  the  dynamical  simula¬ 
tion  of  the  system.  The  F-matrlx  of  this  model  with  the  augmented 
states  for  the  simulation  of  the  errors,  is  given  by 
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where  wx  -  n  cos  L  and  uz  =  -n  sin  L,  with  n  the  angular  rate  of  rotation 

of  the  earth;  l  Is  the  latitude;  and  p  represents  the  expression 
wz  *  tan  I- 
K 


(4-8) 


.  The  system  consists  of  three  channels:  the  x-,  y-  and 
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2-channel,  as  shown  by  the  dotted  lines  in  Fig.  14.  These  channels  are 
coupled  by  terms  containing  functions  of  the  angular  velocity,  «,  and/or 
the  latitude,  L.  These  cross-coup! ing  terms  between  the  different 
channels  of  the  system  (Fig.  14)  are  generally  much  smaller  than  the 
other  terms.  This  fact  is  used  for  a  simplification  of  the  Kalman 
filter  equations.  If  all  cross-coupling  terms  are  ignored,  the  model 
in  Fig.  15  is  obtained.  The  upper  left  part  of  the  new  system  matrix 
F  becomes 


The  order  of  the  states  in  the  state  vector  is  changed  to 

XT  =  [6Ry,  6Ry,  *x,  6Rx,  6Rx,  *y,  *2]  (4-9) 

A  new  F, i -matrix  is  obtain  by  rearranging  th°  ruW',  and  columns 
accordingly. 


(4-10) 
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The  state  vector  matrix  quatlon  can  now  be  decomposed  Into  three 
Independent  sets  of  differential  equations.  If  the  states  to  simulate 
the  errors  are  Included,  the  three  state  vectors  for  thec^  thre® 


Independent  systems  are 


*1  s  [6V  5V  V  V  CX’  vy] 


(4-1  la) 


A  =  C«Rv.  7v*  v] 


x*  y’  x  yw  xJ 


x3  B  CV 


(4-1  lb) 
(4-llc) 


The  states  to  simulate  the  velocity  measurement  errors,  v  and  v  ,  are 

*  y 

coupled  to  their  corresponding  channel  through  the  measr*,ement  matrix. 
The  F-matrices  corresponding  to  the  three  state  vectors  are 


0  0  -g  1  0 

0  1/R  0  0  1 

o  o  o  -e7y  o 

0  0  0  0  -6  , 


(4-1 2a ) 


1  0 


0  -1/R  0 


0  'evx  0 

0  0  ***y 

0  0  0 


(4-1 2b) 


0  1 


0  0 


(4-1 2c ) 
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The  computer  time  and  memory  requirements  are  considerably  reduced  when 
the  system  in  Fig.  14  is  decomposed  into  a  set  of  three  independent 
systems.  The  equations  for  the  Kalman  filter  can  be  programmed  much  more 
efficiently  this  way  on  a  special-purpose  computer, 

A  third  model  for  the  system  is  shown  in  Fig.  16.  This  model  can  be 
decomposed  into  a  set  of  two  independent  systems.  Without  the  augmented 
states  to  simulate  the  errors,  the  state  vector  and  the  corresponding 
system  matrix  F^  are  given  by 
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(4-13) 


(4-14) 


The  system  matrix  can  be  decomposed  along  the  dotted  lines.  The  states 
to  simulate  the  random  inputs  can  be  added  to  the  appropriate  channel 
in  analogy  to  Eq.  (4-8)  to  obtain  a  full  system  matrix. 


C.  Analysis  of  Three  Estimation  Schemes 

The  models  of  the  system  in  Figs.  14,  15  and  16  will  be  referred  to 
as  models  A,  8  and  C.  Model  A  simulates  the  actual  dynamics  of  the  system 
very  closely  and  will  therefore  be  used  to  simulate  the  system.  The 
following  three  estimation  schemes  have  been  used 


i 
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1.  Optimum  Kalman,  using  fiiodel  A  for  the  simulation  of  the  system 
and  for  the  Kalman  filter; 

2.  Sub-optimum  Kalman,  using  model  B  for  the  filter;  and 

3.  Sub-optimum  Kalman,  using  model  C  for  the  filter. 

The  reference  Information  for  the  estimation  schemes  Is  assumed  to 

consist  of  velocity  measurements  only.  The  filter  system  matrix  of 

model  A  Is  stored  according  to  Eq.  (4-8).  The  filter  system  matrices 

for  model  B  and  C  are  stored  the  same  way,  l.e.,  as  a  14  x  14  matrix 

with  the  states  In  the  same  order  as  In  Eq.  (4-4).  The  change  in  the 

order  of  the  states  mentioned  In  the  previous  section  Is  merely  a  way 

to  Illustrate  how  the  F-matrlx  can  be  decomposed  Into  smaller  matrices 

In  order  to  be  programmed  more  efficiently  on  a  special-purpose  digital 

computer.  For  example,  model  A  contains  fourteen  states,  all  of  which 

are  coupled,  either  through  the  system  matrix  or  the  measurement 

matrix,  and  therefore  all  the  states  have  to  be  estimated  simultaneously. 

However,  model  B  contains  only  twelve  states  to  be  estimated.  The  two 

states  In  the  z-channel,  $2  and  e,z.  are  not  affected  by  the  optimum  gain 

as  they  are  uncoupled  from  the  observable  states,  6R  and  <5R  .  The 

X  y 

resulting  twelve-state  system  can  be  decomposed  into  two  uncoupled 
systems  each  with  six  states,  of  which  the  system  matrices  are  given  by 
Eqs.  (4-1 2a )  and  (4-1 2b) .  The  optimum  gain  and  the  estimate  can  then 
be  calculated  for  each  of  these  systems  separately.  This  results  in 
storing  more  matrices,  but  of  smaller  size,  than  with  the  system  composed 
of  all  the  states  as  would  be  done  for  model  A.  Model  C  contains 
fourteen  states  to  be  estimated  since  the  z-channel  Is  coupled  with  the 
x-channel.  The  x-  and  y-channels  are  uncoupled,  so  that  the  system  can 
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be  decomposed  Into  two  systems,  one  with  six  states  for  the  y-channel, 

an<  the  other  with  eight  states  to  simulate  the  x-  and  z-channels. 

Here  also  the  new  estimate  and  the  optimum  gain  can  be  calculated 

separately  for  each  of  the  two  systems. 

The  external  measurements  used  are  the  velo.  s  In  the  x-  and 

y-dlrections,  6R  and  6R  .  Realizing  that  the  measurement  errors  have 
x  y 

to  be  added  to  the  output  also,  Eq.  (2-lb)  becomes 


y 


6  R  +  v 

r  - 
V 

X  x 

■f 

_  X 

6ft  +  v 

V 

y  y 

y 

L»  — * 

(4-15) 


where  v  and  v  are  the  "white  noise"  components  of  the  measurement 
x  y 

errors  and  v  and  v  are  the  bias  terms  of  the  measurement  errors.  The 
x  y 

measurement  matrix  M  for  the  system  will  be 


M 


00100000000 
0001  0000000 


0  1 
0  0 


0 

1 


(4-16) 


The  output  vector  for  the  filter  In  Eq.  (2-23)  Is  likewise  obtained  and 
results  in  the  same  measurement  matrix  as  for  the  system  In  Eq.  (4-16). 

Table  I  contains  the  values  for  the  different  parameters  used  and 
their  values  when  converted  into  the  units  of  feet,  seconds  and  radians. 


TABLE  I  System  Parameters 


n 


6 


15°/hr 


45c 


1/4  hrs 


-1 


1/2  hrs 


T^T 


32.2  ft/sec 


2.1 xlO'ft 


.  944x1 0"4rad/sec 


.785  rad 


6.95xl0”5sec'1 


.139  sec 


-1 


-4  -1 

*  fi  cos  L  *  .666x10  sec 


8  -fi  sin  L  =  -.666*10"4  sec“^ 
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The  Initial  conditions  for  the  error  In  estimation,  x,  are  contained 
In  Table  II  In  addition  to  their  values  In  the  unite  ' v  feet,  seconds 
and  radians. 


TABLE  II  Initial  Conditions 


KS1 

\,y 

di 

x,y 

*z 

°7x,y 

°ex,y.z 

°vx,y 

3000  ft 

1.  ft/sec 

10  min 

10° 

20  sec 

.01o/hr 

.3  ft/sec 

3.*103 

1. 

2.916*10“3 

.175 

3.12xl0“3 

4.925x10"® 

.3 

ft 

ft/sec 

rad 

rad 

rad  ft/sec2 

rad/sec 

ft/sec 

The  Initial  condition  for  the  covariance  matrix  of  the  error  In  estimation 
P(0)  Is  obtained  by  squaring  the  values  In  Table  II.  The  covariance  of 
the  random  driving  terms  is 

Cov(wy)  *  2eva2  *  2.7l*10"9  rad  ft2/sec4 

Cov(wc)  *  2e£o2  *  3.37*10‘19  rad/sec3 


The  values  for  e  ,  p  ,  o„  ano  o  are  obtained  from  Tables  i  and  II.  The 
diagonal  of  both  the  covariance  matrix  for  the  system  and  the  filter  Is 
obtained  from  the  above  values  for  the  random  driving  terms,  and  all  the 
off-diagonal  terms  are  zero  as  these  random  signals  are  uncorrelated. 

The  covariance  of  the  "white  noise"  error  components  of  the  measure¬ 
ments  Is  given  by 


Cov(v  )  «  Cov(v  )  ■  1/4  ft2/$ec2 
a  y 

The  covariance  matrix  of  these  errors  Is  given  by 


C  - 


0 

.25 


For  the  control  matrices  and  A?  the  Identity  matrix  is  used.  The 
Initial  conditions  for  Cov(z)  can,  with  Eq.  (4-45)  and  Eq.  (4-46),  be 
represented  by 


The  first  half  of  the  diagonal  terms  are  equal  to  the  diagonal  elements 
of  P(0).  The  terms  In  the  second  half  of  the  diagonal  and  all  off-diagonal 
terms  are  equal  to  zero. 

For  the  simulations,  a  measurement  time  T  ■  60  sec  was  used.  The 
number  of  updates  KT  «  2.  The  number  of  measurements  KTF  ■  30,  correspond¬ 
ing  to  a  final  time  of  t  *  1/2  hour. 

0.  Results  and  Cownents 

The  results  of  the  simulations  with  the  different  models  are  plotted 
in  Figs.  17  through  20.  The  figure'  contain  the  estimation  errors  In 
6Rx,  and  for  all  three  models.  The  y-channel  results  were 

not  plotted  since  they  are  for  all  three  models  very  much  like  the 
results  for  the  x-channel  of  the  optimum  case.  One  can  notice  that 
model  A  gives  the  best  estimate,  closely  followed  by  the  results  with 
model  C.  The  estimate  obtained  with  model  B  Is  far  worse.  However,  It 
Is  misleading  In  this  case  to  compare  the  three  estimation  schemes  when 
the  measurement  time  Is  the  same  for  all  three  cases.  The  calculation 
of  the  optimum  gain  takes  much  longer  with  the  complicated  model  A  than 
with  the  far  less  complicated  model  B.  Therefore  with  models  B  and  C 
more  measurements  can  be  taken  and  more  estimates  calculated  In  the  same 
period  of  time  than  with  model  A.  The  more  measurements  taken,  the 


Fig.  17  Error  in  estimation  of  the  position  in  the  x-direction. 


faster  the  steady  state  Is  reached,  and  due  to  the  small  correlation 
time  of  the  errors,  the  steady  state  may  be  smaller. 
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In  order  to  get  a  better  Idea  about  the  trade-offs,  Table  III 
contains  for  each  of  the  three  models: 

1.  The  amount  of  storage  necessary  for  the  matrices; 

2.  The  time  Involved  to  calculate  a  new  estimate;  and 

3.  The  steady  state  for  the  states  of  the  system  after  30  minutes. 
The  amount  of  storage  given  in  Table  III  does  not  Include  the  mernury 
necessary  for  the  Instructions.  The  matrices  Include:  AF,  AQ,  PHI,  RK, 
AH,  AC,  PI ,  AK,  T1  and  T2. 


TABLE  III  Summary  of  the  Results 


Model  A 

Model  B 

Model  C 

Memory  necessary  to  store 
all  matrices  (words) 

1432 

484 

728 

Time  of  calculations 

1. 

.16 

.26 

Steady  state  after  30  minutes 

6Rx  In  103  ft 

3.02 

* 

17.5 

3.05 

<5Ry  In  103  ft 

3.05 

3.10 

3.05 

<5&x  In  ft/sec 

.394 

21.4 

.395 

6Ry  In  ft/ sec 

.373 

.811 

.374 

$x  In  inln 

.192 

.407 

.338 

<j>y  In  min 

.245 

16.6 

.362 

<}>2  In  degrees 

.058 

9.9 

_ 

.060 

"k 

This  state  does  not  reach  steady  state,  but  will  continue  to 
grow  (see  Fig.  17)  due  to  velocity  error. 
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In  Chapter  IV,  Section  C  it  was  already  shown  that  by  Ignoring  the  cross¬ 
coupling  terms  of  the  system  the  system  could  be  decomposed  into  two 
different  uncoupled  models  of  the  system,  resulting  In  a  smaller  storage 
space  required  for  the  various  matrices.  Besides  gaining  a  considerable 
amount  of  storage  space,  especially  with  large  systems,  there  is  also  a 
great  amount  of  computer  time  gained.  Most  of  the  calculations  consist 

of  multiplications  of  matrices.  To  multiply  two  n  x  n  matrices,  there 

3 

are  n  multiplications  needed.  If  the  dimension  of  the  matrices  is 
reduced  by  a  factor  2,  the  amount  of  calculation  is  reduced  by  a  factor 
23  =  8. 

Besides  the  fact  that  some  matrices  are  reduced  in  size  by 
decomposing  the  system  matrix,  the  transition  matrix,  PHI,  and  the 
covariance  matrix  of  the  error  term  for  the  random  Input  of  the  filter, 

RK,  usually  converge  more  rapidly.  This  is  an  important  factor  when 
these  two  matrices  have  to  be  re-calculated  at  every  time  a  new  estimate 
is  calculated.  This  is  the  case  when  the  measureme ’t  time  Is  not  constant, 
and/or  when  the  system  matrix  in  the  filter  is  time-varying.  It  should 
be  noted  here  that  the  convergence  of  PHI  and  RK  depends  highly  on  the 
arrangement  of  the  rows  and  columns.  The  order  of  the  states  in  the 
state  vector  should  be  arranged  In  such  a  way  that  all  elements  of  the 
system  matrix  are  as  close  as  possible  to  the  diagonal  in  order  to 
obtain  rapid  convergence. 

The  calculation  of  the  tine  involved  to  calculate  a  new  estimate 
is  derived  by  assuming  that  most  of  the  computer  time  Is  taken  up  by 
multiplying  matrices  — In  particular,  the  matrices  with  the  largest 
dimensions.  These  are:  AF,  AQ,  PHI,  RK,  PI,  T1  and  T2.  By 
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decomposing  the  system  matrix,  AF,  all  these  matrices  are  reduced  In 
size  In  the  same  way  as  AF.  With  model  A,  the  multiplication  of  two 
matrices  will  Involve  14^  -  2744  calculations.  The  system  matrix  of 
model  B  Is  split  Into  two  6x6  matrices,  therefore  the  equivalent 

3 

multiplication  takes  2*6  ■  432  calculations.  The  system  matrix  of 

model  C  Is  split  Into  an  8  x  8  matrix  and  46x6  matrix.  The  equivalent 

3  3 

multiplication  here  Involves  8  +  6  *  728  calculations.  It  Is  assumed 
that  the  number  of  these  calculations  Is  proportional  to  the  time  of 
calculating  a  new  estimate.  In  Table  III,  the  time  to  calculate  a  new 
estimate  for  model  A  Is  taken  to  be  1.0,  and  all  other  times  are  relative 
to  this  model. 

Apparently  an  estimator  using  model  C  In  the  Kalman  filter  Is  the 
most  desirable,  as  It  can  be  programmed  quite  efficiently  by  decomposing 
It  Into  two  uncoupled  systems,  and  still  the  system  reaches  steady  state 
In  a  short  time.  The  reason  that  model  B  exhibits  a  strong  growth  of  the 
errors  In  the  x-channel  is  because  the  azimuth,  <t>2»  has  been  uncoupled  In 
this  model  which  makes  <pz  unobservable.  Therefore  4>z  Is  unaffected  by 
the  optimum  gain.  Model  B  may  be  better  suited  for  use  when  more  states 
are  observable. 


APPENDIX  A 


The  Matrix  Exponential  Equation 

An  Important  subroutine  In  the  program  Is  the  matrix  exponential 
(MEXP)  subroutine.  This  routine  utilizes  the  system  matrix  F  to  calculate 


the  transition  matrix  which  is  defined  by 
$(At)  =  eFAt 


(A-l ) 


To  evaluate  this  equation,  $  is  put  in  a  series  form  which  can  be 
written  as 

2  3 

$(At)  =  I  +  F  At  +  F^  ^  ^  +  •  •  •  ( A— 2 ) 

This  series  converges  [9]  and  can  easily  be  programmed  on  a  digital 
computer.  The  flow  diagram  of  this  subroutine  Is  shown  In  Fig.  (A-l). 


where  w(t)  is  a  random  noise  process,  a  relation  has  to  be  obtained 
describing  x(t)  at  any  Instant  of  time  as  a  function  of  an  initial 
condition  x(0).  Due  to  the  random  noise,  a  statistical  description  of 
the  system  is  necessary.  The  covariance  matrix  of  the  states  Is  given 
by 

P(t)  =  E[x(t)  xT(t)J 
with  E[x(t)]  =  0  if  E[x(0)]  *  0. 


To  generate  the  covariance  matrix  P(t)  as  a  function  of  time  and  an  initial 
condition.  Its  differential  equation  Is  to  be  examined  as  follows 


P(t)  -  E[x(t)  xT(t)]  +  ECx(t)  xT(t)] 

Substituting  the  system  equation  yields 

E[x(t)  xT(t)]  *  E[{F  x(t)  +  w(t)}  xT(t)] 

-  F  P(t)  +  E[w(t)  xT(t)] 

Likewise, 

E[x(t)  iT(t)]  -  P(t)  FT  +  E[x(t)  wT(t)] 

Noting  that  x(t)  can  be  written  as 

x(t)  =  $(tQ,  t)  x(tQ)  +  |  *(t,  t)  w( t )  dx 

Zo 

and  substituting  Into  Eq.  (A-4)  yields 

E[x(t)  wT(t)]  -  ECT *( tQ,  t)  x(tQ) }  wT(t)j  + 

+  E[{  1  $(x,  t)  w(x )  dx)  wT(t}] 

t  itQ 

*  E[|  $(  x,  t)  w(t)  wT(t)  dx] 

rt0 

•  «(t.  t)  E[w(t)  wT(t)]  dx 
JtO 

According  to  Eq.  (2-5) 

E[w(x)  wT(t)]  -  Q  6(t  -  x) 

Substituting  into  Eq.  (A-5)  yields 
T  f  T 

E[x(t)  w'(t)]  =  ♦(t,  t)  Q  6(t  -  x)  dx 

Jto 

Fig.  (A-2)  shows  the  Impulse  function  d(t  -  x). 


(A-3) 


(A-4) 


(A-5) 


(A— 6 ) 
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With  a  pulse  width  e,  the  Impulse  function  Is  only  non-zero  between 
t  -  1/2  e  and  t  +  1/2  e.  Therefore,  the  lower  Integration  limit  in 
Eq.  (A-6)  can  be  changed  from  t  to  t  -  1/2  e  without  changing  the 
result.  Because  e  Is  Infinitely  small, 

*(t,  t)  -  $(t,  t)  «  I 

As  the  Impulse  is  only  Integrated  over  half  Its  area,  from  t  -  1/2  e 
to  t,  the  final  result  of  Eq.  (A-5)  contains  the  factor  1/2, 

E[x(t)  wT(t)]  -  1/2  Q 

Substituting  this  result  Into  Eq.  (A-3)  yields  a  simplified  matrix 
Rlcattl  equation 

P(t)  -  P(t)  FT  +  F  P(t)  +  Q  (A-7) 

The  solution  to  this  differential  equation  Is  obtained  by  the 
following  method.  By  direct  substitution  It  can  be  shown  that 

P(t)  -  [o21  +  e22  P(0)][en  +  e12  P(O)]"1  (A-8) 

where 

e( t)  •  z  o(t)  (A-9) 
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®(0) 


°1  T^°J  J  _Q12_f°_^ 

©21(°)  |  ©22 


,(0) 


z  * 


T  1 

■Ft  I  0 
I - 1 - j 

Q  1  F  , 

*  t  -I 


(A-10) 


(A-ll ) 


Proof:  Rearranging  Eq.  (A-8)  and  taking  the  time  derivative  yields: 


P(t)[©n  +  ©12  P(0)]  ■  [e21  +  o22  P(0)] 

P(t)[©n  +  e12  P(0)3  ♦  P(t)[3n  +  o12  P(0)]  •  [e21  +  022  P(0)]  (A-12) 
From  Eq.  (A-9) 

°U  ■  Z11  Q11 
®12  *  ° 

(A-13) 

e21  J  Z21  eil  +  z22  °21 
©22  «  z22  o22 

where  *12  "  0  according  to  Eq,  (A-ll),  and  0,2  ■  0  which  becomes  apparent 
later  on  in  Eq.  (A-16)  when  the  expression  for  e(t)  Is  found.  Substituting 
these  results  Into  Eq.  (A-12)  yields 

Ht)  -  -P(t)  z^  +  z21  +  z22Cs21  +  ©22  P(0)3  0^J 

Inserting  the  values  for  z  from  Eq.  (A-ll) 

P(t)  «  P(t)  FT  +  Q  +  F  P(t)  (A-14) 


which  proves  that  both  equations  are  equivalent. 

In  order  to  find  an  algorithm  to  program  Eq.  (A-8),  the  series  method 
Is  used.  From  Eqs.  (A-9)  and  (A-ll) 
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e(t)  ■  e 


LQ 


i 

i 


(A-15) 


Using  the  matrix  exponential  method  to  solve  for  (t)  yields 

[i  |  <fl  f-FT|  dl  f-FT  |  Of  t2  f-FT  1  Of  t3 

ip  t  ij  [q  ■  fJ  |_q  j  fJ  21  |_q  ;  fJ  3r 

Performing  the  matrix  multiplications  yields 

fi  o]  Pft!  o]  f(-FT)2  j  0 1  t2 
0(0  ■  — 1-~  + - J — t  ♦  — ■» — yr  + 

L°  i  !J  LQ !  FJ  +FQ  I  F  J 


(-ft)3 


F(FQT-QFT)-(FQT-QFT)FT| 


t\ 

3T  + 


(A-16) 


By  noting  how  the  series  progresses*  one  can  easily  obtain  simple 
expressions  for  ©12  and  ©22  as  follows 


©n(t) 

©22^^ 


■FTt 


■Ft 


(*-’)T 


©12(t)  ■  0 

With  these  results,  Eq.  (A-8)  can  be  written  In  a  simpler  form 

P(t)  ■  •  P(0)  *T  +  e21  ejj  (A-17) 

©21  can  be  easily  found  by  using  the  matrix  exponential  method  for 
solving  o  -  ezt.  This  method,  however,  takes  a  substantial  amount  of 
time  on  the  computer  due  to  the  large  dimension  of  z;  therefore  another 
method  will  be  used.  In  the  following  derivation  It  turns  out  that  for 
012  e!l  a  rither  simple- to-program  expression  can  be  found.  Multiplying 
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the  series  of  ©21  and  o~J  gives  the  result 

02i  Gii  ■  Qt  +  ^pT  +  fQT)  Jy  +  L(QFt  +  ftq)ft  +  f(qft  +  fqt)t]  *!♦.. 

v  I 

This  equation  can  be  written  In  arother  form,  realizing  that  Q  Is 
symmetric,  or  Q  «  QT,  which  makes  QFT  -  ( FQ ) T ,  so  that  FQT  +  QFT  1S 
again  synmetric. 

°21  ell  “  Q*  +  (FQ  +  (FQ)T)  +  [F(fq  +  (FQ)T)  + 

+  (F(FQ  +  (FQ)T)}T]  jj-  +  ■“  (A-18) 

This  series  Is  easy  to  program  with  a  simple  Iteration  routine  shown  In 
Fig.  (A-3),  with  e21  oj]  *  R^. 
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APPENDIX  B 


This  appendix  contains  a  listing  of  the  sub-optimum  Kalman  filter 
estimation  program  explained  In  Chapter  III. 


program  estim 

O 1 MENS  ION  AF  <  20  *  20 ) «  BF  <  25  «  25  >  *  AQ  <  20  *  20 ) • BQ  <  25  *  25 ) *  PH I <  20  *  20 ) « 

I  0PM I <25 •25) «  RK <  20 • 20 ) «  BRK  <  29 • 25 ) ♦ AM  < 10 .20 ) *BM < 1 0. 25 > ♦ AK <20 » I O ) • 
2AC< 10. 10) .Pi  <20  *20  >  *BC  <10*10) « COVU < 45*45 ) »COVZ  <45*45  > • AA  <20*20 ) * 
3BA ( 25*  20 ) *TA  <  25*  25 ) • TC  <  25*25 ) *0H  <45*45) 

COMMON  T l <50.25) .T2 <50.25 ) «T3 <50*25 >  *T4 <25.25 ) *T5 <25. 25) 
EQUIVALENCE  <AF.BPMI )«  <  AO  «  BRK ) * <BF*T4). <BQ*T5) *  <  T 1 «  TA ) * <T2*TC) 

1  FORMAT  <815*  3E 10*5) 

3  FORMAT!/*  PHI*) 

4  FORMAT {/*  BPHI#) 

5  FORMAT!/*  RK*) 

6  FORMAT!/*  BRK*> 

7  FORMAT <#!  N  NB  M  KT  KT F  KMAX  I  OUT  KA 1  T  AE  RE*) 

00  222  .10 

PRINT  7 

READ  1  .N.N0.M.KT.KTF.KMAX. I OUT  * KA I « T « AE « RE 
IF<EOF*60)  223.29 

28  PRINT  l «N»NB»M.KT.KTF.KMAX, I  OUT »KA I » T ♦ AE • RE 
0*T/FL0AT  <KT  ) 

NNB*N*N8 

CALL  AINPUT<AF*BF*A0.BQ*AM,BM.AC.BC«P1 *C0V2 * AA *QA .N.NB *M« NNB *KA I ) 
PRINT  3 

CALL  MEXPtQ.AF.AF.Tl .T2 »T3,PhI .N.KMAX.AE.RE*  I) 

PRINT  5 

CALL  MEXP<  0*  AF  *  AQ  *  T 1 *T2*T  3*  RK «  N .KMAX  « A£ t RE*  2) 

PRINT  4 

CALL  M£XP<  O.BF  *8F . T 1 »T2  *  T3.BPHI » NB  *KMAX  « AE«RE *  1) 

PRINT  6 

CALL  M£XP<0*BF«BQ.T1 . T2 . T3 ♦ BRK » NB * KMAX . AE * RE »  2) 

CALL  ERCOV  < PH  I .BPHI .RK . BRK • AM. BM» AK. AC ♦ 0C *PI . 0H.C0VU.C0V2.AA.BA. TA 

i .tc.n.nb.m.nnb.kt.ktf. iout.o) 

222  CONTINUE 

223  CONTINUE 
END 
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SUBROUTINE  A  I  NPUT ( AF<BF<AO<BO< AM <2M<AC<BC<P1  <C0VZ<AA<BA<N<N8<M< 
lNNO<XA! > 

DIMENSION  AF <N«N ) <BF (NB<NB ) < AQ< N<N) <BQ<NB<N8>  •  AM  (M<N  )  »BM  (M<  NB  >  < 
t AC(M*M ) <P)  (N<N>  »COVZ(NNB*NNB >  <BC (M»M >  *  AA (N<N) <BA(NB<N> 

1  FORMAT (BE I 0*5) 

2  FORMAT (1X«12E11<4> 

3  FORMAT (X*  AF») 

4  FORMAT (X*  BF*) 

5  FORMAT (X*  AO*} 

6  FORMAT (X*  BO*) 

7  FORMAT (X*  AM*) 

B  FORMAT ( X*  BM*) 

9  FORMAT (X*  AC*) 

12  FORMAT (X*  BC*) 

10  FORMAT (X*  PI*) 

11  FORMAT (X#  COVZ*) 

13  FORMAT (X*  AA*| 

14  FORMAT (X*  BA* ) 

OO  20  I  *1  <N 

20  REAO  I<  (AF(I <J)<J*1<N> 

PRINT  3 

CALL  DRUK(AF«N<N) 

OO  21  I  *  1  *  NB 

21  REAO  1*  (BF (  I  <  J) <  J*1 *NB ) 

PRINT  4 

CALL  DRUK(BF.NB»NB ) 

PRINT  5 

GO  T0<22<23>KAI 

22  CALL  AOMAO<N<N<  1) 

READ  1 <  <A0( !<!)<!•! <N> 

PR I NT 2 < <AQ( I < I )< 1*1 <N) 

PRINT  6 

CALL  AOMBO<NB<N8<  1) 

REAO  1 < (B0( I < I )< I«|«NB> 

PRINT2<  <BQ( I • I >  < I  *  I < NB ) 

GO  T0  29 

23  00  24  I  * 1 <N 

24  REAO  I < <A0( I < J)< J*1 < 1 ) 

OO  29  I *2<N 

f  I  *  I  -I 

00  29  J*1 < I  I 

25  AQ( J< 1 )*A0( I < J) 

CALL  ORUK(AO<N<N) 

PRINT  6 

00  26  1*1 «NB 

26  REAO  1 < (BQ< I < J) < J*1 < ! ) 

OU  27  !«2«NB 

I  1*1-1 

00  27  J*1 < 1 ! 

27  B0( J< I )*B0( M J) 

CALL  DRUK(0Q<NB<N8> 


29  DO  30  t«|,M 

30  RE4D  | «  <AM< I ,j) ,j«| «N> 
PRINT  7 

CALL  DRUK(AM*M,N) 

00  31  I-i  ,M 

31  READ  1,  <8M( I ,NB) 
PRINT  8 

CALL  ORUK ( BN iMi NS  I 
00  32  t«l,w 

-*2  READ  I*  (AC(  I  « J)  «j») 

PRINT  9 

CALL  ORUK (AC  <M«M 1 
DO  33  t  «  1 

33  READ  1«  (BC( 1 .J) ,j«| ,«> 
PRINT  12 

CALL  ORUK(BC.M.M) 

PRINT  10 
GO  TOC  3S«40 1KA I 
35  CALL  AO I (Pi «  N  ,N  «  1) 

READ  1  i  ( Pi  (  I  *  I  >  ,  j  ■  j  j 
PRINT2,(Pi (i,i 
CAt-*-  A01  (COVZ.NNB.NNB.  1) 
READ  1 « (COVZ < I « I ) < I  a i «NN8 1 
PRINT  1 1 

PRINT2, (COVZ  < I < I  )« I«| «NN8) 
GO  To  49 

40  00  41  I»l tN 

41  READ  I,(Pl(l,j,,j,|f j, 

00  43  I«1  ,Nf^a 

43  READ  1 . (C0V2( I «j).j-l , I ) 

00  45  I«2«N 
I  I  ■  I  - 1 

00  45  J«I « 1  I 
*S  PI ( J. l)»Pl » 1 « j> 

CALL  DRUK(P1«n»N) 

00  47  I«2«NNS 
IUI-1 

00  47  J«1 ,11 
47  C0V2(J* I )»COVZ( I , j> 

PRINT  II 

CALL  ORUK (COVZ * NN8 t NNB  ) 

49  CALL  AOI ( AA  *N«N«  1  > 

READ  t«(AA(l,l),i.|'N) 
PRINT  13 

PRINT  2«  (AA  (  I  «  I  |  ,  i  ,|  ,(vj  j 
00  50  I »i <n8 

50  REAO  1,  (BA ( I <j)«ja|«N) 
PRINT  14 

CALL  ORUK(BA»NB»N) 

RETURN  S  END 
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SUBROUTINE  MEXp<D*F*A»Tl«T2«EXpTA»T3«N»KMAX»AE«Re*KLM> 

Ot  MENS  I  ON  A<N«NMF(NiN)  «EXPTA(N*N)«Tt  <  N.N  )  •  T«  <  N  .  N  )  «  T3  <  N.  N  ) 

DOUBLE  PRECISION  Tl  *T2*EXPTA«PK»T*TK 
2  FORMAT ( *  NUMBER  OF  ITERATIONS  K**14) 

9  FORMAT!/*  MAX*  NUMBER  OF  ITERATIONS  EXCEEDED*) 

T»D 

PK*1 .0 
00  I  0  J* I «N 
00  10  I  *  1  «N 
T 1  (  I  •  J)»T*A< I «J> 

10  EXPTAd  *J)*T1  <  I.J) 

GO  TO  (  1 4  1 1 8  )  KLM 

14  00  19  1*1 «N 

15  EXPT  A ( I «  I  )  *EXPTA  < I « I )+ 1 *0D 
18  PK*PK+I*D 

TK*T/PK 
DO  30  1*1 *N 
00  30  J*1 iN 
T2< I • J )»0«D 
00  30  L*1 iN 

30  T2< 1 «J)*T2( l «J>TF( I*L)*Tl  (C* J) 

36  00  20  J*1 «N 

00  20  1 *1 «N 
GO  T0<  38  «  39 )  KLM 

30  T1  < I  * J)*T2( I « J)*TK  %  GO  TO  20 
39  T1!I«J)*(T2(I*J)  ♦  T2<J«I)>*TK 

20  EXPTAU  «U»«EXPTA<  I  «  J)  *  TK1«J) 

60  DO  70  J* 1 *N 
DO  70  1*1 *N 

ER*DABS<  T|( I »  J )/ ( AE+RE*DABS  <EXPT  A ( I • J) ) ) > 

IFtER.GT.  1.0  >  GO  TO  80 
70  CONTINUE 

GO  TO  110 

80  IF<PK.LT.KMAX )  GO  TO  10 
PRINT  5  *  STOP 

110  K«PK 

PRINT  2.K 
DO  120  I  *  1 « N 
DO  120  J* 1 *N 
120  T3< I • J)«EXPTA < I . J  ) 

CALL  DRUK  (T3.N.N) 

RETURN  S  END 

SUBROUTINE  ERCOV  <PH1 «BPM! «RK .0RK » AM.BM. AK ♦ AC.BC .PI «  OH « COVU ♦ COVZ  «  A A 
1 .BA.TA.TC.N.NBtM.NNB.KT.KTF* IOUTtT) 

DIMENSION  PHI (N.N).BPHl ( NB • NB ) ♦ T A ( NB *NB > « TC IN. NB ) 

1  *RK (N«N ) . BRK INB.NB) «AM I M.N) .BM<  M.NB) »AK (N.M).AC (M.M) .BCIM.M) 

2  «  A A (N*N) «BA ( NB  «N ) «  OH  INN'S  *  NNB ) < COVU < NN0 » NNB >< C0V2 ( NNB « NNB >  *P1 IN»N> 
COMMON  T 1 (25*  25 ) »T2 I2L*  2L )  ;  T3 1 29*25 ) *T4  <  25*25 ) •T8(25«2S) .T5I50.50) 

1 .T7I50.25).T9<50«25)»T6'30.SO> 


EQUIVALENCE  ITl «T5). IT6.T7.T8) 

2  FORMAT l//*  MEASUREMENT  number* iai 

3  FORMAT!///*  TIME  »*F11.4) 

C  MAKE  A A* A A- I 

00  5  I  *  I  .N 

5  AA ( 1 • t  )«AA  < I « I)-l • 

00  25  l  !»l .KTF 
TIME«T#FLOATI I  I )*FLOAT(KT) 

CALL  AO I  ( OH • NN8  *  NNB «  II 
CALL  AO  II COVU. NNB* NNB.  i) 

00  10  I  *  I  i NB 
00  10  J*t«NB 
OH  I I . J )«BPHI  l  I  .  J  I 
10  COVU ( I • U ) «8RK ( 1 «  J ) 

OO  15  I«*t.N 
OO  15  J»1  .N 

15  0H( I*NB« J+NB >=PH! I I i J ) 

00  18  Kl >1 .KT 
C  UPDATE  PI 

CALL  ABTACIPHI .Pi .PHI .T1 .T2.N.N.N.N.  2) 

CALL  MATADDIT2.RK.P1 .N.N.N) 

C  UPDATE  COVARIANCE (2) 

CALL  ABTAC(0H«C0V2.0H.T5»T6«NNe.NNB»NNB.NNB»  2) 
CALL  MATaoDIT6»COVU«COVZ.NNB.NNB.NNB> 

T I MEA  =  T I ME-T  *FLOAT  IKT-K 1  I 
PRINT  3.TIMEA 

17  CALL  OUT(Pi .COV2.T1 .N.NB.NNB. IOUT ) 

18  CONTINUE 
PRINT  2.1! 

CALL  filteriam.ak.pi »AC«N«M» 

CALL  MATMPV(AK.BM.T8.N.N.M.M.NB> 
call  MATMPY(BA.T8. ta.ns.nb.n.n.nb 1 
CALL  MATMPY(AK.AM.TC.N.N.M.M.N) 

DO  65  I « I . N0 
65  TAI I . I )«TA! I « I J-l • 

DO  70  I«1 »N 

TCI  I . I  )«TC( I .11-1* 

DO  70  J«1  « N 
70  TCI  I  .JI—TCI  I  .J) 

CALL  MATMPY (BA.TC.T2.NB.NB.N.N.N) 

CALL  'IATMPY  IAA.TC.T4.N.N. N.N.N) 

CALL  MATMPY IAA.T0.TC.N. N.N.N. NB I 

CALL  SUM I TA . T2 . TC . T4 . OH . NB  «  N «  NB ♦ N . NNB . NNB I 

CALL  A8TACIAK.BC.AK.T2.Tl .N.M.M.N.  2) 

CALL  SUMS  I BA. . A A . . T7.NB . N . N. . NNB  »  N ) 

CALL  A8TACIT7.T1  «T7«T9«  COVU*  NNB . N.N.NNB  *  2> 

CALL  ABTac<0H.C0VZ.0H.T5*T6.NNB»NNB»NNB*NNB«  2) 
CALL  MAT ADD  <T6» COVU. C0V2. NNB. NNB. NNB ) 

CALL  OUT  I  Pi .COVZ.T1 .N.NB.NNB. IOUT  > 

25  CONTINUE 

RETURN  *  END 
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SUBROUTINE  FlLTER<AM»AK«Pl *AC*N*M) 

DIMENSION  AM(M«N)*AK!N«M)*P| <N«N)»ACIM*M) 

COMMON  T1 (25*23) ♦ T 2 ( 25 •  2S  >  «  T3 ( 75 *  SO  I 
9  FORMAT!/*  AKA) 

C  CALCULATE  K 

20  CALL  ABTAC<AM»P1 *AM»T1 •T2«M*N«N*M»  2) 

CALL  MATAOO(T2«AC«T1 «M«M«M) 

N0»0 

call  MATINV1T 1 «M*Tl «N0»DET«M| 

CALL  ABT<P1 *AM*T2*N*N*M) 

CALL  MATMPY  <  T2 ♦ T I •  AK  *N«N*  M*  M«M ) 

C  CALCULATE  P| 

call  ABTAC<AK«AM«P| «Tl «T2'N*M*N*N«  3) 

CALL  MATSUB!P| *T2«P1 «N*N*N) 

40  PRINT  9 

CALL  DRUKIAK*N*M) 

RETURN  A  END 

SUBROUTINE  OUT (PI «  COVZ *  T  «N *N8  *NNB • IOUT ) 

DIMENSION  PI CN*N > *COVZ ( NNB*NNB ) »T(NB«NB) 

2  FORMAT!/*  C0V2* ) 

3  FORMAT!/*  SQUARE  ROOT  OF  UPPER  DIAGONAL  TERMS  OF  COVZ.*) 
S  FORMAT! 1X*12E1 I *4 ) 

8  FORMAT!/*  PI*) 

IF! IOUT.LE.1 >  GO  TO  19 
PRINT  S 

CALL  DRUK IP1 *N«N) 

PRINT  2 

CALL  DRUK < COVZ «NN8 « NNB > 

19  PRINT  3 

DO  20  I -1 «N8 

20  T  < I • 1 > "SQRTF ! COVZ 1  I t I ) ) 

PRINT  3* <T! I * 1) . I«l *N8 I 
RETURN  »  END 

SUBROUTINE  DRUK (A  *N«M ) 

DIMENSION  AlNtM) 

101  FORMAT!*  TRANSPOSE*) 

102  FORMAT! |X« 12E1 1*4) 

lF(N.GT.M)  GO  TO  2 
00  1  1 • 1 »N 

1  PRINT! 02* ( A  < I • J ) • J* 1 «M ) 

RETURN 

2  PRINT  101 
OO  3  !■! *M 

3  PRINT  1 02* ( A< J* I ) » J«!  *N  ) 

RETURN  A  END 
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SUBROUTINE  SUM! A.B.CiO*  T« ll<I2*Jl«U2«I12*J12) 

DIMENSION  A ( I  1 • J |  |  •  8  <  I  1  • J2  |*C(!2tJl  >»D(I2»U2>»TI||2»UI2) 
DO  20  J«I ♦  J2 
oo  is  i ■ i « n 

IS  T  < I  «J+J|  )«B< I «J> 

OO  20  t-t«I2 
20  T  < I  +  1 1 «  J+Jl ) "D ( I »  J ) 

ENTRY  SUMB 
DO  10  J«1 « JI 
00  5  I  «  1  «  I  1 

5  T  (  I  «  J  I  » A  (  I  •  J  ) 

00  10  I ■ 1 «  12 
10  T(|+li«J)>C(!*J) 

RETURN  *  ENO 

SUBROUTINE  AOI<A«N«M»  KK ) 

01 MENS I  ON  A ( N • M ) 

OO  10  I«1 iN 
OO  10  J«1 *M 
10  A<1*J>«0«0 

IPIKK.EO* 1 )  RETURN 
DO  20  I *1 »N 
20  A(I«I)*1*0 

RETURN  S  ENO 

SUBROUTINE  AB T (A.8*T«K«L»M) 

DIMENSION  A  (K  *L ) «B I M  *L 1 •  T <K«M) 

DO  1  J«1.M 

DO  1  1 • 1 «  K 

T  <  1  • J)»0.0 
00  1  K 1  *  1  «L 

1  T< I *U)«T t I « J )+A ( I «K1>*8 < J»KI > 

RETURN  S  END 

SUBROUTINE  ABTAC<A»B«C*  T*Y«K«L»MtN»KK ) 

DIMENSION  AIK  <L) t  B  (  L  «  M  )  «  C  <  N  *  M  ) ♦  T  <  K  ♦  M  ) iY I K  t  N  ) 

C  ABAT  KK«2  Y»A*B*AT 

C  ABCT  KK»3  Y»A*B*CT 

CALL  MATMPY(A*B»T»K«K»L»L«M1 
GO  TO ( 9» 2 ♦ S )  KK 

2  00  4  I  • 1  *  K 
00  4  J«I »N 
Y( I «U)«0.0 
00  3  Kl >1 «M 

3  YI I « J)«Y< l • JM-T < I «K1 )*C < J«K1 ) 

4  Y I J  » I  J  » Y ( I »J> 

GO  TO  9 

5  CALL  ABTI T«C» Y»K«M«N> 

9  RETURN  %  ENO 
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